#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "dsp_common.h"

#include "dspm_mult.h"
#include "dsps_fft2r.h"



extern void xt_iss_profile_disable();
extern void xt_iss_profile_enable();

bool dsp_is_power_of_two(int x)
{
    return (x != 0) && ((x & (x - 1)) == 0);
}

int dsp_power_of_two(int x)
{
    for (size_t i = 0; i < 16; i++)
    {
        x = x >> 1;
        if(0 == x) return i;
    }
    return 0;
}

#define test_size 128

static float data[test_size*2];
static float check_data[test_size*2];
static float data_test[test_size*2];

void test_fft2r()
{

    int N = sizeof(data) / sizeof(float) / 2;
    int check_bin = 32;
    float check_ampl = 2000;
    for (int i = 0 ; i < N ; i++) {
        data[i * 2 + 0] = check_ampl * sinf(M_PI / N * check_bin * 2 * i) / (N / 2);
        data[i * 2 + 1] = 2;
    }
    for (int i = 0 ; i < N*2 ; i++) {
        check_data[i] = data[i];
        data_test[i] = -1;
    }

    // Init FFT tables
    esp_err_t ret = dsps_fft2r_init_fc32(NULL, 4096);
    if (ret  != ESP_OK)
    {
        printf("Not possible to initialize FFT. Error = %i \n", ret);
        return;
    }
    int N_check = N;
        // table = (uint16_t *)dsps_fft2r_rev_tables_fc32[6];
        // table_size = dsps_fft2r_rev_tables_fc32_size[6];

xt_iss_profile_enable();
    dsps_fft2r_fc32_ae32(data, N_check);
//    dsps_bit_rev_lookup_fc32_ae32(data, N_check, dsps_fft2r_rev_tables_fc32[6]);
    dsps_fft2r_fc32_aes3_(check_data, N_check, dsps_fft_w_table_fc32);
//    dsps_bit_rev_lookup_fc32_ae32(data, N_check, dsps_fft2r_rev_tables_fc32[6]);
xt_iss_profile_disable();

    for (int i=0 ; i< N_check ; i++)
    {
        if (abs(check_data[i] - data[i]) == 0)
        {
            printf("Data[%i] =%8.4f, %8.4f, %f \n", i, data[i], check_data[i], check_data[i] - data[i]);
        }
        else printf("ERROR: Data[%i] =%f, %f, %f \n", i, data[i], check_data[i], check_data[i] - data[i]);
    }


    printf("Test Pass!\n");
}

